#include <mc_ros/controller_ros_node.h>


ControllerRosNode::ControllerRosNode(tf2_ros::Buffer* tf_buffer):
    robotModelPtr_(nullptr),tfBuffer_(tf_buffer),
    goalPosePtr_(nullptr),fusionDataPtr_(nullptr),odomDataPtr_(nullptr),
    costmap_local_ptr_(nullptr),costmap_global_ptr_(nullptr){
    // 机器人模型初始化
    // robotModelConfigFile_ = ros::package::getPath("mc_ros") + "/config/robot_model.yaml";
    robotModelConfigFile_ = ros::param::param<string>(
        "robot_params_file", "/home/orangepi/trobot/trobot_ros1/config/params/orangepi/robot_params.yaml");
    robotModelPtr_ = boost::make_shared<ModelBase>(robotModelConfigFile_);
    mcVisualizationPtr_ = boost::make_shared<MCVisualization>();
    mcVisualizationPtr_->setRobotModel(robotModelPtr_);

    // 初始化
    // mapHandlerPtr_ = boost::make_shared<MapBase>();
    // costmapGlobalPtr_ = boost::make_shared<costmap_2d::Costmap2DROS>("global_costmap", *tfBuffer_);
    // costmapLocalPtr_ = boost::make_shared<costmap_2d::Costmap2DROS>("local_costmap", *tfBuffer_);


    int global_planner_type = ros::param::param<int>("/global_planner/planner_type", 0);
    switch(global_planner_type){
        case 0:
            LOG(WARNING) << "Path planner is AstarPlanner.";
            globalPathPlannerPtr_ = boost::make_shared<AstarPlanner>();
            break;
        case 1:
            LOG(WARNING) << "Path planner is JPSPlanner.";
            globalPathPlannerPtr_ = boost::make_shared<JPSPlanner>();
            break;
        default:
            LOG(WARNING) << "Path planner is AstarPlanner.";
            globalPathPlannerPtr_ = boost::make_shared<AstarPlanner>();
            break;
    }
    int local_controller_type = ros::param::param<int>("/local_controller/controller_type", 0);
    switch(local_controller_type){
        case 0:
            LOG(WARNING) << "Controller is DWAController.";
            localControllerPtr_ = boost::make_shared<DWAController>();
            break;
        case 1:
            LOG(WARNING) << "Controller is PurePursuit.";
            localControllerPtr_ = boost::make_shared<PurePursuit>();
            break;
        case 2:
            LOG(WARNING) << "Controller is Stanley.";
            localControllerPtr_ = boost::make_shared<Stanley>();
            break;
        case 3:
            LOG(WARNING) << "Controller is LQR.";
            localControllerPtr_ = boost::make_shared<LQR>();
            break;
        default:
            LOG(WARNING) << "Controller is DWAController.";
            localControllerPtr_ = boost::make_shared<DWAController>();
            break;
    }  

    processorP2P_ = boost::make_shared<ProcessorP2P>(globalPathPlannerPtr_,
        localControllerPtr_, mcVisualizationPtr_);

    // ros 
    mapBevPub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/map_bev", 1);
    mapDilatePub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/map_dilate", 1, true);
    // trackedPathPub_ = nh_.advertise<nav_msgs::Path>("/tracked_path", 1);
    // robotModelMarkerPub_ = nh_.advertise<visualization_msgs::MarkerArray>("/robot_model", 1);
    costmapGlobalSub_ = nh_.subscribe("/global_costmap/costmap", 10, &ControllerRosNode::CostmapGlobalCallback, this);
    costmapLocalSub_ = nh_.subscribe("/local_costmap/costmap", 10, &ControllerRosNode::CostmapLocalCallback, this);

    costmapGlobalPub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/global_costmap", 1);
    costmapLocalPub_ = nh_.advertise<nav_msgs::OccupancyGrid>("/local_costmap", 1);
    rvizPoseSub_ = nh_.subscribe("/move_base_simple/goal", 10, &ControllerRosNode::RvizPosesCallback, this);
    mapSub_ = nh_.subscribe("/map", 10, &ControllerRosNode::MapCallback, this);
    // lidarScanSub_ = nh_.subscribe("/scan_raw", 10, &ControllerRosNode::LidarCallback, this);
    fusionDataSub_ = nh_.subscribe("/fusion_bev", 10, &ControllerRosNode::FusionDataCallback, this);
    odomSub_ = nh_.subscribe("/odom", 10, &ControllerRosNode::OdometryCallback, this);

    controllerCmdService_ = nh_.advertiseService("/controller_server", &ControllerRosNode::ControllerCmdCallback, this);

    // 循环控制
    Loop();
}


ControllerRosNode::~ControllerRosNode(){

}

 // 地图回调函数
void ControllerRosNode::MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg){
    LOG(INFO) << "Receive a map. width=" << msg->info.width 
        << ", height=" << msg->info.height
        << ", resolution=" << msg->info.resolution;

    // costmapGlobalPtr_->setMapData(msg);
    // costmapLocalPtr_->setMapData(msg);

    occupancyGrid_ = msg;
    // 路径规划器设置地图
    // globalPathPlannerPtr_->setMap(msg);
    // // 简单的地图膨胀 (膨胀半径为0.25)
    // mapHandlerPtr_->SimpleDilateOccupancyGrid(0.25);
    // // 发布膨胀后的地图
    // auto map_ptr = boost::make_shared<nav_msgs::OccupancyGrid>();
    // if(mapHandlerPtr_->getMap(map_ptr, msg->header.frame_id)){
    //     mapDilatePub_.publish(*map_ptr);
    // }
}

// 全局代价地图回调函数
void ControllerRosNode::CostmapGlobalCallback(const nav_msgs::OccupancyGrid::Ptr& msg){
    costmap_global_ptr_ = boost::make_shared<nav_msgs::OccupancyGrid>(*msg);
    globalPathPlannerPtr_->setMap(costmap_global_ptr_); 
}

// 局部代价地图回调函数
void ControllerRosNode::CostmapLocalCallback(const nav_msgs::OccupancyGrid::Ptr& msg){
    costmap_local_ptr_ = boost::make_shared<nav_msgs::OccupancyGrid>(*msg);
    localControllerPtr_->updateCostmapData(msg);
}

// 二维雷达回调函数
void ControllerRosNode::LidarCallback(const sensor_msgs::LaserScan::Ptr& msg){
    // costmapLocalPtr_->setLidar2DScanData(msg);
}

// Rviz位姿回调函数
void ControllerRosNode::RvizPosesCallback(const geometry_msgs::PoseStampedConstPtr& msg){
    goalPosePtr_ = msg;
    ROS_INFO("[RvizPosesCallback] get pose, x=%0.3f, y=%0.3f, yaw=%0.3f.", 
        msg->pose.position.x, 
        msg->pose.position.y,
        tf2::getYaw(msg->pose.orientation)
    );

    // 开始进入P2P任务
    processorP2P_->setGoal(*msg);
    processorP2P_->setEvent(StateMachine::Event::EV_WORKING);    
}

// 融合数据回调函数
void ControllerRosNode::FusionDataCallback(const sensor_msgs::PointCloud2::Ptr& msg){
    fusionDataPtr_ = msg;
    // costmapGlobalPtr_->setPointCloud2Data(fusionDataPtr_);
    // costmapLocalPtr_->setPointCloud2Data(fusionDataPtr_);
}

// 里程计回调函数
void ControllerRosNode::OdometryCallback(const nav_msgs::Odometry::Ptr& msg){
    odomDataPtr_ = msg;
}


// 服务回调函数
bool ControllerRosNode::ControllerCmdCallback(tr_msgs::ControllerCmd::Request& req, tr_msgs::ControllerCmd::Response& res){
    LOG(INFO) << "option: " << req.option;

    switch(req.option){
        case req.PAUSE:
            processorP2P_->setEvent(StateMachine::Event::EV_PAUSE);
            break;
        case req.RESUME:
            processorP2P_->setEvent(StateMachine::Event::EV_RESUME);
            break;
        case req.CANCEL:
            processorP2P_->setEvent(StateMachine::Event::EV_CANCEL);
            break;        
        default:
            break;
    }
    res.response.success = true;
    res.response.message = "success";
    return true;
}


// 循环控制
void ControllerRosNode::Loop(){
    processorP2P_->setEvent(StateMachine::Event::EV_IDLE);
    ros::Rate controller_rate(20);  //20Hz

    nav_msgs::OccupancyGrid local_map;
    nav_msgs::OccupancyGrid global_map;
    while(!ros::isShuttingDown()){
        controller_rate.sleep();
        ros::spinOnce();
        LOG(INFO) << "--------------------------------";

        ros::Time now = ros::Time::now();
        // if(costmapGlobalPtr_){
        //     if((now - global_map.header.stamp).toSec() > 3.0){
        //         costmapGlobalPtr_->updateCostmap();
        //         costmapGlobalPtr_->getOccupancyGrid(global_map);
        //         costmapGlobalPub_.publish(global_map);

        //         // 更新地图
        //         auto global_map_ptr = boost::make_shared<nav_msgs::OccupancyGrid>(global_map);
        //         cout << "globalPathPlannerPtr_ setMap................................" << endl;
        //         globalPathPlannerPtr_->setMap(global_map_ptr);
        //     }
        // }

        // if(costmapLocalPtr_){
        //     if((now - local_map.header.stamp).toSec() > 0.1){
        //         costmapLocalPtr_->updateCostmap();
        //         costmapLocalPtr_->getOccupancyGrid(local_map);
        //         costmapLocalPub_.publish(local_map);
        //     }
        // }

        // 阻塞状态检测
        // 获取机器人当前位姿
        geometry_msgs::PoseStamped global_robot_pose;
        if(!localControllerPtr_->getRobotPose(tfBuffer_, global_robot_pose)){
            LOG(WARNING) << "can not get robot pose!";
            // 进入阻塞状态  待补全
            continue;
        }
        if(!odomDataPtr_){
            LOG(WARNING) << "can not get odom data!";
            // 进入阻塞状态  待补全
            continue;            
        }
        localControllerPtr_->updateRobotPose(global_robot_pose);

        // 发布运动轨迹
        mcVisualizationPtr_->publishTrackingPose(global_robot_pose);

        // 发布机器人模型
        mcVisualizationPtr_->publishRobotModel("base_link");

        // 状态机控制
        ROS_INFO_STREAM("curr_state is " << processorP2P_->getCurrStateName());
        cout << "global_robot_pose: " << global_robot_pose.pose.position.x << "  " 
            << global_robot_pose.pose.position.y << "  " 
            << tf2::getYaw(global_robot_pose.pose.orientation) << endl;
        
        processorP2P_->updateRobotState(global_robot_pose, *odomDataPtr_);
        processorP2P_->handleEvent();
    }
}


int main(int argc, char** argv){
    ros::init(argc, argv, "controller_ros_node");

    string glog_dir = ros::param::param<string>("/glog_dir", "/home/xxty/user_config/logs");
    common::glog::init(glog_dir, "controller_ros_node");

    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    LOG(INFO) << "controller_ros_node start...";

    tf2_ros::Buffer tfBuffer(::ros::Duration(10));
    tf2_ros::TransformListener tfListener(tfBuffer);
    ControllerRosNode controller_ros_node(&tfBuffer);
    ros::spin();

    common::glog::shutdown();
    return 0;
}